/*
 * equalizer.c
 * Created on August 30, 2007; 4:07 PM
 * by VRamadurai
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>

#include "HsTypes.h"
#include "PhyDebug.h"
#include "PhyStruct.h"
#include "PmdGlobals.h"
#include "PhyConstants.h"
#include "equalizer.h"

void
eq_buffer_init(struct HS_Phy11bStr  *HS_Phy11bPtrTh)
{
    hs_int32_t i;

    for(i=0; i<PREAMBLESYNCSIZE; i++){
        HS_Phy11bPtrTh->preamble_buffI[i] = 0;
    }

    HS_Phy11bPtrTh->eq_in_index = 0;
}

void
set_eq_ref_samples(struct HS_Phy11bStr *HS_Phy11bPtrTh, hs_int32_t offset)
{
    hs_int32_t eq_ref_samples_skip;
    eq_ref_samples_skip = (offset)*RX_DOWNSAMPLE_SIZE;
    HS_Phy11bPtrTh->eq_in_index = eq_ref_samples_skip - N_POST;
}

/** Sync with a portion of the 128bit preamble sequence **/
/** Once a match is found, it returns the symbol position in
    the input buffer of the matched sequence **/
/** This will tell the exact bit position of the preamble in
    the input buffer **/
hs_int32_t
preamble_sync(
                hs_int16_t * restrict inR,
                hs_int16_t * restrict preamble_ref
            )
{
    hs_int32_t i, k;
    hs_int32_t corrsumr;
    hs_int16_t sumr;
    hs_int16_t PreambleCorr[20];
    hs_int16_t maxval;
    hs_int32_t maxpos;


    for (k=0; k < 20; k++)
    {
        PreambleCorr[k] = 0;
        corrsumr = 0;
        for (i=0; i < CORR_LEN; i++)
        {
            corrsumr += inR[i]*preamble_ref[k*11+i];
        }
        sumr = corrsumr >> 15;

        PreambleCorr[k] = (sumr*sumr)>>15;
    }

    maxval = 0;
    maxpos = 0;

    for(i=0; i<20; i++){
        if(PreambleCorr[i] > maxval){
            maxval = PreambleCorr[i];
            maxpos = i;
        }
    }
    return(maxpos);
}
#if 1
hs_int32_t
preamble_sync_intr(
hs_int16_t * restrict inR,
hs_int16_t * restrict inQ,
hs_int16_t * restrict preamble_ref
)
{
hs_int32_t i, k;
hs_int32_t corrsumrI;
hs_int32_t corrsumrQ;
hs_int16_t sumrI[32];
hs_int16_t sumrQ[32];
hs_int16_t PreambleCorr[32];
hs_int32_t maxval;
hs_int32_t maxpos;
hs_int32_t count;

__sb_rbroad(&inR[CORR_LEN], 0, 0);
__sb_rbroad(&inQ[CORR_LEN], 0, 0);
__sb_rbroad(&sumrI[16], 0, 0);
__sb_rbroad(&sumrQ[16], 0, 0);

for (k=0; k < PREAMBLE_NUM_CORR; k++)
{
	corrsumrI = 0;
	corrsumrQ = 0;
	for(i=0; i<6; i++){
	corrsumrI = __sb_rmulreds(corrsumrI, &inR[i*16], &preamble_ref[k*11+i*16]);
	corrsumrQ = __sb_rmulreds(corrsumrQ, &inQ[i*16], &preamble_ref[k*11+i*16]);
	}
	sumrI[k] = corrsumrI >> 16;
	sumrQ[k] = corrsumrQ >> 16;
}
__sb_rmuls(PreambleCorr, sumrI, sumrI);
__sb_rmacs(PreambleCorr,sumrQ,sumrQ,PreambleCorr);
__sb_rmuls(PreambleCorr+16, sumrI+16, sumrI+16);
__sb_rmacs(PreambleCorr+16,sumrQ+16,sumrQ+16,PreambleCorr+16);

maxval = 0;
maxpos = 0;
count = 0;
__sb_rsearchmax(&maxval,&count,&maxpos,maxval,count,maxpos,PreambleCorr);
__sb_rsearchmax(&maxval,&count,&maxpos,maxval,count,maxpos,PreambleCorr+16);

return(maxpos);
}

#endif
#if 0
hs_int32_t
preamble_sync_intr(
                hs_int16_t * restrict inR,
                hs_int16_t * restrict preamble_ref
            )
{
    hs_int32_t i, k;
    hs_int32_t corrsumr;
    hs_int16_t sumr[32];
    hs_int16_t PreambleCorr[32];
    hs_int32_t maxval;
    hs_int32_t maxpos;
    hs_int32_t count;

    __sb_rbroad(&inR[CORR_LEN], 0, 0);
    __sb_rbroad(&sumr[16], 0, 0);

    for (k=0; k < PREAMBLE_NUM_CORR; k++)
    {
    	corrsumr = 0;
    	for(i=0; i<6; i++)
    		corrsumr = __sb_rmulreds(corrsumr, &inR[i*16], &preamble_ref[k*11+i*16]);
    	sumr[k] = corrsumr >> 16;
    }
    __sb_rmuls(PreambleCorr, sumr, sumr);
    __sb_rmuls(PreambleCorr+16, sumr+16, sumr+16);

    maxval = 0;
    maxpos = 0;
    count = 0;
    __sb_rsearchmax(&maxval,&count,&maxpos,maxval,count,maxpos,PreambleCorr);
    __sb_rsearchmax(&maxval,&count,&maxpos,maxval,count,maxpos,PreambleCorr+16);

    return(maxpos);
}
#endif
void
AdaptiveEqTapsInit(
                hs_int16_t *w_LEri
              )
{
    hs_int32_t i;

    for (i=0;i<32;i++)
    {
        w_LEri[i] = 0;
    }
}

void
AdaptiveEqInit(
                struct HS_Phy11bStr  *HS_Phy11bPtrTh,
                hs_int16_t *inbuff_LEri
              )
{
    //equalizer init is only performed per burst:
    hs_int32_t i;

    HS_Phy11bPtrTh->eq_count = 0;

    for (i=0;i<2*(N_LE+RXSIZE);i++)
    {
        inbuff_LEri[i] = 0;
    }
}


void
frac_energy(
            struct HS_Phy11bStr *HS_Phy11bPtrTh,
            hs_int16_t * restrict buff_LEr,
            hs_int16_t * restrict buff_LEi
        )
{
    #define ENERGY_SCALE    4
	#define ALPHA_FIXED		8 //(0.5*(1<<4))//(ALPHA_FLOAT*(1<<ENERGY_SCALE))

    hs_int16_t *buffI;
    hs_int16_t *buffQ;
    hs_int32_t pos;
    hs_int16_t temp;
    hs_int32_t eq_escale;
    hs_int32_t shift;
    hs_int32_t energy;
    hs_int32_t i;

    buffI = buff_LEr;
    buffQ = buff_LEi;

    //NLMS normalization energy: linear part:
    energy = 0;
    for(i=0; i<11; i++)
    {
        energy += (buffI[i]*buffI[i] + buffQ[i]*buffQ[i]);
    }

    /*Finding out Most Significant One Bit */
    pos = __sb_clz(energy&0x7FFFFFFF)-1;
    pos = 31 - pos;

    shift = (1<<pos);
    temp = (ALPHA_FIXED*shift)/energy;
    eq_escale = (pos+ENERGY_SCALE) - 15;
    HS_Phy11bPtrTh->eq_energy = temp << (15-eq_escale);
    //__putstring("energy = %d\n", HS_Phy11bPtrTh->eq_energy);
}


void
AdaptiveEqData(
            hs_int16_t * restrict buff_LEr,
            hs_int16_t * restrict buff_LEi,
            hs_int16_t * restrict w_LEr,
            hs_int16_t * restrict w_LEi,
            hs_int16_t * restrict output_eqr,
            hs_int16_t * restrict output_eqi,
            hs_int32_t in_len
         )
{
    hs_int32_t i;
    hs_int32_t index;

    hs_int32_t yr, yi;
    hs_int16_t yr_s, yi_s;

    /** 11MHz equalizer **/
    for (index=0; index<in_len; index++)
    {
        yr = 0;
        yi = 0;
        for (i=0;i<N_LE;i++)
        {
            yr += (buff_LEr[index+i]*w_LEr[i] + buff_LEi[index+i]*w_LEi[i]);
            yi += (buff_LEi[index+i]*w_LEr[i] - buff_LEr[index+i]*w_LEi[i]);
        }

        yr_s = yr >> 15;
        yi_s = yi >> 15;

        output_eqr[index] = yr_s;
        output_eqi[index] = yi_s;
    }

    //at the end of each equalizer run, move the very last N_PRE+N_POST samples
    //in the equalizer buffer to the beginning of the buffer:
    for (i=0;i<(N_PRE+N_POST);i++)
    {
        buff_LEr[i] = buff_LEr[in_len+i];
        buff_LEi[i] = buff_LEi[in_len+i];
    }
}

hs_int32_t
AdaptiveEqPreamble(
            hs_int16_t * restrict buff_LEr,
            hs_int16_t * restrict buff_LEi,
            hs_int16_t * restrict w_LEr,
            hs_int16_t * restrict w_LEi,
            hs_int16_t * restrict RefEq,
            hs_int16_t energy_fixed,
            hs_int32_t eq_in_index,
            hs_int32_t eq_count,
            hs_int32_t in_len
         )
{
    //Adaptive equalizer:
    //Use preamble sequence for filter tap adaptation via complex NLMS algorithm.
    //The after preamble, the filter taps are frozen.
    //DFE part removed.
    //Assume that the equalizer will process the whole input synchronized burst.
    //%%%%%%%%%%%%%%% equalizer start %%%%%%%%%%%%%%%
    //linear equalizer part: precursor and postcursor:

    hs_int16_t er;
    hs_int16_t ei;

    hs_int32_t i;
    hs_int32_t index;

    hs_int32_t yr, yi;
    hs_int16_t yr_s, yi_s;

    /** 11MHz equalizer **/
    for (index=0; index<in_len; index++)
    {
        if(eq_count % 4 == 0){

            yr = 0;
            yi = 0;
            for (i=0;i<N_LE;i++)
            {
                yr += (buff_LEr[index+i]*w_LEr[i] + buff_LEi[index+i]*w_LEi[i]);
                yi += (buff_LEi[index+i]*w_LEr[i] - buff_LEr[index+i]*w_LEi[i]);
            }

            yr_s = yr >> 15;
            yi_s = yi >> 15;

            //error:
            er = RefEq[eq_in_index] - yr_s;
            ei = 0 - yi_s;

            //filter update: linear part:
            for (i=0;i<N_LE;i++)
            {
                hs_int16_t tr;
                hs_int16_t ti;

                hs_int16_t temp1 = ( buff_LEr[index+i]*er + buff_LEi[index+i]*ei );
                hs_int16_t temp2 = ( buff_LEi[index+i]*er - buff_LEr[index+i]*ei );

                tr = ((temp1*energy_fixed) >> 15);
                ti = ((temp2*energy_fixed) >> 15);

                w_LEr[i] += tr;
                w_LEi[i] += ti;
            }
        }

        eq_in_index++;
        eq_count++;
    }

    //at the end of each equalizer run, move the very last N_PRE+N_POST samples
    //in the equalizer buffer to the beginning of the buffer:
    for (i=0;i<(N_PRE+N_POST);i++)
    {
        buff_LEr[i] = buff_LEr[in_len+i];
        buff_LEi[i] = buff_LEi[in_len+i];
    }

    return eq_count;
}
